#define USAGE \
  "USAGE: sonarobstacleavoid [-h <host>] [-p <port>] [-m]\n" \
  "       -h <host>: connect to Player on this host\n" \
  "       -p <port>: connect to Player on this TCP port\n" \
  "       -m       : turn on motors (be CAREFUL!)"

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <string.h>
#include "gnuplot_i.hpp" //Gnuplot class handles POSIX-Pipe-communikation with Gnuplot
#include "infos_robot.h"
#include "planification.h"

   
# define SIMULATION 1
# define XREP  -13;
# define YREP  -13;
# define nombre_policier 4

#define debutx_voleur 20
#define debuty_voleur -10

/* Creation du robot accessible depuis toutes les fonctions */
using namespace PlayerCc;

#if  SIMULATION == 1
PlayerClient robot (gHostname, gPort);
Position2dProxy pp_pos(&robot,0);   
Position2dProxy pp_commande(&robot,0); 
OpaqueProxy op(&robot); 
int coeff=1;
int appel = 0;
#else
/*
printf("ici");
PlayerClient robot (gHostname, gPort);
Position2dProxy pp_pos(&robot,1);   
Position2dProxy pp_commande(&robot,0);
OpaqueProxy op(&robot); 
int coeff=4;
*/
#endif 


PlayerClient robot_v1 (gHostname, gPort1);
OpaqueProxy op_v1(&robot_v1); 



PlayerClient robot_v2 (gHostname, gPort2);
OpaqueProxy op_v2(&robot_v2); 


PlayerClient robot_v3 (gHostname, gPort3);
OpaqueProxy op_v3(&robot_v3); 


PlayerClient robot_v4 (gHostname, gPort4);
Position2dProxy pp_voleur(&robot_v4,0); 
OpaqueProxy op_v4(&robot_v4); 






player_opaque_data_t opData;
uint8_t * opGetData; 

pthread_mutex_t mutex_position;  
std::vector<double> x,y,xv,yv,xv2,yv2;
Gnuplot g1("lines");
Gnuplot g2("lines");


double distance(double A[2], double B[2])
{
double distance;
distance = sqrt((A[0]-B[0])*(A[0]-B[0]) + (A[1]-B[1])*(A[1]-B[1]));
return distance;
}
  
 
double distanceToSegment(double A[2],double B[2],  double P[2]) 
{
  double sx,sy,ux,uy,dp,sn2,ah2,un2;
  
  if (A[0] == B[0] && A[1] == B[1]) 
  {
    return distance(P,A);
  }
  sx = B[0] - A[0];
  sy=B[1] - A[1];
  ux = P[0] - A[0];
  uy = P[1] - A[1];
  dp =sx*ux+sy*uy;

  if (dp<0) 
  {return distance(A,P);}

  sn2 = sx*sx+sy*sy;
  if (dp>sn2) 
  {return distance(B,P);}

  ah2 = dp*dp / sn2;
  un2=ux*ux+uy*uy;
  return sqrt(un2-ah2);
}



void decompose(double intermediaire, double result[6])
{
  
	double entree;
	entree = intermediaire;
	if(entree > 0)
	{
		result[0] = 0.0;
	}
	else
	{
		result[0] = 1.0;
		entree = -1*entree;	
	}
	
	if(entree >=0 && entree <10)
	{
	  result[1] = floor(entree);
	  
	  entree = (entree - result[1])*10;
	  result[2] = floor(entree);

	  entree = (entree - result[2])*10;
	  result[3] = floor(entree);
	  
	  entree = (entree - result[3])*10;
	  result[4] = floor(entree);
	  
	  result[5] = 1; 
	}  
	else if(entree >= 10 && entree <100)
	{
	result[1] = floor(entree*0.1);
	entree = (entree * 0.1 - result[1])*10;
	result[2] = floor(entree);
	
	entree = (entree - result[2])*10;
	result[3] = floor(entree);
	
	entree = (entree - result[3])*10;
	result[4] = floor(entree);
	
	result[5] = 2;
	}
	else if(entree >= 100 && entree <1000)
	{
	result[1] = floor(entree*0.01);
	entree = (entree * 0.01 - result[1])*100;
	
	result[2] = floor(entree*0.1);
	entree = (entree * 0.1 - result[2])*10;
	
	result[3] = floor(entree);
	
	entree = (entree - result[3]);
	result[4] = floor(entree);
	
	result[5] = 3;
	}
	else
	{

	result[1] = 0;
	result[2] = 0;
	result[3] = 0;
	result[4] = 0;
	result[5] = 0;
	}
return;
}

void wait_for_key ()
{
    std::cout << std::endl << "Press ENTER to continue..." << std::endl;

    std::cin.clear();
    std::cin.ignore(std::cin.rdbuf()->in_avail());
    std::cin.get();
    return;
}




double equation_segmentx(double point1[2], double point2[2], double evaluation)
{
   double a,b;
   double resultat;
   a = (point1[1] - point2[1])/(point1[0] - point2[0]);
   b = point1[1] - a * point1[0];   
   
   if(evaluation >= min(point1[0],point2[0]) && evaluation <= max(point1[0],point2[0])) 
   {
     resultat = a * evaluation + b;
   }
   else
   {
     resultat=0;
   }
  return resultat;
}


double equation_segmenty(double point1[2], double point2[2], double evaluation)
{
   double a,b;
   double resultat;
   a = (point1[1] - point2[1])/(point1[0] - point2[0]);
   b = point1[1] - a * point1[0];   
   
   if(evaluation >= min(point1[1],point2[1]) && evaluation <= max(point1[1],point2[1]) && a!=0) 
   {
     resultat = (evaluation - b)/a;
   }
   else
   {
     resultat=0;
   }
  return resultat;
}



void cooperation(double objectif_courant[3])
{
  std::vector<double> x_voleur, y_voleur;
	std::vector<double> x_cercle, y_cercle, x_vision, y_vision,x_obstacle,y_obstacle,x_disque,y_disque;
	int indice_obstacle_locaux=0;
	int appartient = 1;
	int inter=0;
	int cas_problem[1000][4];
	double vision = 10;
	double pas = PAS_METRIQUE;
	double minimum=1000;
	double obstacle_locaux[nb_obstacle_rect][4];
	double d1,d2,d3,d4,distance1,distance2,distance3,distance4,distance_au_goal,resultat,evaluation,point1[2], point2[2];
	double xrep = XREP;
	double yrep = YREP;
	double position_voleur[2];
	double distance_blocage = 3;
	int nombre_de_point_voleur;

	robot_v4.Read();
  position_voleur[0] = -xrep + debutx_voleur + pp_voleur.GetXPos();
  position_voleur[1] = -yrep + debuty_voleur + pp_voleur.GetYPos();

  
 for(int i=0 ; i<nb_obstacle_rect ; i++)
  {
    point1[0]=obstacle_rect[i][0];
    point1[1]=obstacle_rect[i][1];
    point2[0]=obstacle_rect[i][0];
    point2[1]=obstacle_rect[i][3];
    distance1 = distanceToSegment(point1,point2,position_voleur); 
    
    point1[0]=obstacle_rect[i][0];
    point1[1]=obstacle_rect[i][3];
    point2[0]=obstacle_rect[i][2];
    point2[1]=obstacle_rect[i][3];
    distance2 = distanceToSegment(point1,point2,position_voleur); 
    
    point1[0]=obstacle_rect[i][2];
    point1[1]=obstacle_rect[i][3];
    point2[0]=obstacle_rect[i][2];
    point2[1]=obstacle_rect[i][1];
    distance3 = distanceToSegment(point1,point2,position_voleur); 
    
    point1[0]=obstacle_rect[i][2];
    point1[1]=obstacle_rect[i][1];
    point2[0]=obstacle_rect[i][0];
    point2[1]=obstacle_rect[i][1];
    distance4 = distanceToSegment(point1,point2,position_voleur); 
    
    if( distance1 <= distance_blocage|| distance2 <= distance_blocage || distance3 <= distance_blocage|| distance4 <= distance_blocage)
    {
    obstacle_locaux[indice_obstacle_locaux][0] = obstacle_rect[i][0];  
    obstacle_locaux[indice_obstacle_locaux][1] = obstacle_rect[i][1];
    obstacle_locaux[indice_obstacle_locaux][2] = obstacle_rect[i][2];
    obstacle_locaux[indice_obstacle_locaux][3] = obstacle_rect[i][3];
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][0]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][1]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][0]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][3]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][2]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][3]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][2]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][1]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][0]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][1]);
    
    obstacle_locaux[indice_obstacle_locaux][0] = obstacle_locaux[indice_obstacle_locaux][0]+1;
    obstacle_locaux[indice_obstacle_locaux][1] = obstacle_locaux[indice_obstacle_locaux][1]+1;
    obstacle_locaux[indice_obstacle_locaux][2] = obstacle_locaux[indice_obstacle_locaux][2]+1;
    obstacle_locaux[indice_obstacle_locaux][3] = obstacle_locaux[indice_obstacle_locaux][3]+1;
    
    indice_obstacle_locaux= indice_obstacle_locaux + 1;
    }
  }
  

	if(indice_obstacle_locaux == 0)
  {
    objectif_courant[0] = position_voleur[0] + distance_blocage  * cos(POS* M_PI/2); ;
    objectif_courant[1] = position_voleur[1] + distance_blocage  * sin(POS* M_PI/2); ;
    objectif_courant[2] = fin[2];
    return;
  }
  
  
  
  /*
  if(x_obstacle.size() !=0)
  {
    g2.set_style("lines");
    g2.plot_xy(x_obstacle,y_obstacle,"obstacles");
  }
  */
  // Generation des points sur le cercle du robot 
  
	//std::cout << "valeur de init" << init[0] << "    " << init[1] << "    " << std::endl;
	for(double i = 0 ; i<2*M_PI ; i=i+2*M_PI/nombre_de_point)
  {  
    x_cercle.push_back(distance_blocage * cos(i) + position_voleur[0]);
    y_cercle.push_back(distance_blocage * sin(i) + position_voleur[1]);
  }
  

	/*
  g2.set_style("lines");
  g2.plot_xy(x_cercle,y_cercle,"horizon de deplacement du robot");
  */
  
  
  x_vision.clear();
  y_vision.clear();
  


  for(double i=0 ; i<x_cercle.size() ; i++)
  {
    for(int j= 0; j<indice_obstacle_locaux ; j++)
    {      
      if((x_cercle[i] <= obstacle_locaux[j][2]) && (x_cercle[i] >= obstacle_locaux[j][0]) && (y_cercle[i] <= obstacle_locaux[j][3]) && (y_cercle[i] >= obstacle_locaux[j][1]))
      {
			appartient = 0;
      }
    }


		
    if(appartient !=0)
    {
		for(int j= 0; j<indice_obstacle_locaux ; j++)
		{  
			point1[0]= position_voleur[0];
			point1[1]= position_voleur[1];
			point2[0]= x_cercle[i];
			point2[1]= y_cercle[i];
		 
			
			evaluation = obstacle_locaux[j][0];
			resultat = equation_segmentx(point1,point2,evaluation);			
			if(resultat <= obstacle_locaux[j][3] && resultat >= obstacle_locaux[j][1])
			{
			  appartient =0;
			}
			
			evaluation = obstacle_locaux[j][2];
			resultat = equation_segmentx(point1,point2,evaluation);
			if(resultat <= obstacle_locaux[j][3] && resultat >=obstacle_locaux[j][1])
			{
			  appartient =0;
			}
			
			evaluation = obstacle_locaux[j][3];
			resultat = equation_segmenty(point1,point2,evaluation);
			if(resultat <= obstacle_locaux[j][2] && resultat >=obstacle_locaux[j][0])
			{
			  appartient =0;
			}
					
			evaluation = obstacle_locaux[j][1];
			resultat = equation_segmenty(point1,point2,evaluation);
			if(resultat <= obstacle_locaux[j][2] && resultat >= obstacle_locaux[j][0])
			{
			  appartient =0;
			}
						
		}
		if(appartient!=0)
		{
			x_vision.push_back(x_cercle[i]);  
			y_vision.push_back(y_cercle[i]);
		}
    }
    appartient = 1;
  }

 
  //Tracer du disque d'obstacle
  for(int i=0; i<x_vision.size();i++)
  {
    x_disque.push_back(position_voleur[0]);
    y_disque.push_back(position_voleur[1]);
    x_disque.push_back(x_vision[i]);
    y_disque.push_back(y_vision[i]);
  }  
  
  
  
	/*
  if(x_disque.size() !=0)
  {
  g2.set_style("lines");
  g2.plot_xy(x_disque,y_disque,"zone accessible et visible par le robot");
  }
  */
 





  
  //printf("je me dirige vers le points le plus proche de (%f,%f) \n",objectif_courant[0],objectif_courant[1]);
  for(int j=0; j < x_vision.size() ; j++)
  {
    distance_au_goal = (x_vision[j] - objectif_courant[0])* (x_vision[j] - objectif_courant[0]) +  (y_vision[j] - objectif_courant[1])* (y_vision[j] - objectif_courant[1]);
    minimum = min(distance_au_goal,minimum);
    if(minimum ==distance_au_goal)
    {
      inter = j;
    }
  }
  

	nombre_de_point_voleur = x_vision.size();
	
	objectif_courant[0] = x_vision[floor(nombre_de_point_voleur / nombre_policier) * POS];
	objectif_courant[1] = y_vision[floor(nombre_de_point_voleur / nombre_policier) * POS];
	objectif_courant[2] = fin[2];
  
  if(x_vision.size()!=0)
  {
 	objectif_courant[0] = x_vision[floor(nombre_de_point_voleur / nombre_policier) * POS];
	objectif_courant[1] = y_vision[floor(nombre_de_point_voleur / nombre_policier) * POS];
	objectif_courant[2] = fin[2];
  }
  else
	{
  objectif_courant[0] = position_voleur[0];
  objectif_courant[1] = position_voleur[1];
  objectif_courant[2] = fin[2];
 	}




return;
}



/* Fonction qui permet de recuperer les points obstacles dans le voisinage du robot */
void obstacles_voisinages(double init[4], double objectif_int[3], double objectif_courant[3],double distance)
{
std::vector<double> x_cercle, y_cercle, x_vision, y_vision,x_obstacle,y_obstacle,x_disque,y_disque;
int indice_obstacle_locaux=0;
int appartient = 1;
int inter=0;
int cas_problem[1000][4];
double vision = 10;
double pas = PAS_METRIQUE;
double minimum=1000;
double obstacle_locaux[nb_obstacle_rect][4];
double d1,d2,d3,d4,distance1,distance2,distance3,distance4,distance_au_goal,resultat,evaluation,point1[2], point2[2];


g2.reset_plot();

// Recuperation des obstacles locaux
  
  for(int i=0 ; i<nb_obstacle_rect ; i++)
  {
    point1[0]=obstacle_rect[i][0];
    point1[1]=obstacle_rect[i][1];
    point2[0]=obstacle_rect[i][0];
    point2[1]=obstacle_rect[i][3];
    distance1 = distanceToSegment(point1,point2,init); 
    
    point1[0]=obstacle_rect[i][0];
    point1[1]=obstacle_rect[i][3];
    point2[0]=obstacle_rect[i][2];
    point2[1]=obstacle_rect[i][3];
    distance2 = distanceToSegment(point1,point2,init); 
    
    point1[0]=obstacle_rect[i][2];
    point1[1]=obstacle_rect[i][3];
    point2[0]=obstacle_rect[i][2];
    point2[1]=obstacle_rect[i][1];
    distance3 = distanceToSegment(point1,point2,init); 
    
    point1[0]=obstacle_rect[i][2];
    point1[1]=obstacle_rect[i][1];
    point2[0]=obstacle_rect[i][0];
    point2[1]=obstacle_rect[i][1];
    distance4 = distanceToSegment(point1,point2,init); 
    
    if( distance1 <= vision|| distance2 <= vision || distance3 <= vision|| distance4 <= vision)
    {
    obstacle_locaux[indice_obstacle_locaux][0] = obstacle_rect[i][0];  
    obstacle_locaux[indice_obstacle_locaux][1] = obstacle_rect[i][1];
    obstacle_locaux[indice_obstacle_locaux][2] = obstacle_rect[i][2];
    obstacle_locaux[indice_obstacle_locaux][3] = obstacle_rect[i][3];
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][0]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][1]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][0]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][3]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][2]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][3]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][2]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][1]);
    
    x_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][0]);
    y_obstacle.push_back(obstacle_locaux[indice_obstacle_locaux][1]);
    
    obstacle_locaux[indice_obstacle_locaux][0] = obstacle_locaux[indice_obstacle_locaux][0]-distance;
    obstacle_locaux[indice_obstacle_locaux][1] = obstacle_locaux[indice_obstacle_locaux][1]-distance;
    obstacle_locaux[indice_obstacle_locaux][2] = obstacle_locaux[indice_obstacle_locaux][2]+distance;
    obstacle_locaux[indice_obstacle_locaux][3] = obstacle_locaux[indice_obstacle_locaux][3]+distance;
    
    indice_obstacle_locaux= indice_obstacle_locaux + 1;
    }
  }
  

	if(indice_obstacle_locaux == 0)
  {
    g2.reset_plot();
    encombrement = nombre_de_point;
    objectif_int[0] = objectif_courant[0];
    objectif_int[1] = objectif_courant[1];
    objectif_int[2] = fin[2];
    return;
  }
  
  
  
  /*
  if(x_obstacle.size() !=0)
  {
    g2.set_style("lines");
    g2.plot_xy(x_obstacle,y_obstacle,"obstacles");
  }
  */
  // Generation des points sur le cercle du robot 
  
	//std::cout << "valeur de init" << init[0] << "    " << init[1] << "    " << std::endl;
	for(double i = 0 ; i<2*M_PI ; i=i+2*M_PI/nombre_de_point)
  {  
    x_cercle.push_back(vision * cos(i) + init[0]);
    y_cercle.push_back(vision * sin(i) + init[1]);
  }
  

	/*
  g2.set_style("lines");
  g2.plot_xy(x_cercle,y_cercle,"horizon de deplacement du robot");
  */
  
  
  x_vision.clear();
  y_vision.clear();
  


  for(double i=0 ; i<x_cercle.size() ; i++)
  {
    for(int j= 0; j<indice_obstacle_locaux ; j++)
    {      
      if((x_cercle[i] <= obstacle_locaux[j][2]) && (x_cercle[i] >= obstacle_locaux[j][0]) && (y_cercle[i] <= obstacle_locaux[j][3]) && (y_cercle[i] >= obstacle_locaux[j][1]))
      {
			appartient = 0;
      }
    }


		
    if(appartient !=0)
    {
		for(int j= 0; j<indice_obstacle_locaux ; j++)
		{  
			point1[0]= init[0];
			point1[1]= init[1];
			point2[0]= x_cercle[i];
			point2[1]= y_cercle[i];
		 
			
			evaluation = obstacle_locaux[j][0];
			resultat = equation_segmentx(point1,point2,evaluation);			
			if(resultat <= obstacle_locaux[j][3] && resultat >= obstacle_locaux[j][1])
			{
			  appartient =0;
			}
			
			evaluation = obstacle_locaux[j][2];
			resultat = equation_segmentx(point1,point2,evaluation);
			if(resultat <= obstacle_locaux[j][3] && resultat >=obstacle_locaux[j][1])
			{
			  appartient =0;
			}
			
			evaluation = obstacle_locaux[j][3];
			resultat = equation_segmenty(point1,point2,evaluation);
			if(resultat <= obstacle_locaux[j][2] && resultat >=obstacle_locaux[j][0])
			{
			  appartient =0;
			}
					
			evaluation = obstacle_locaux[j][1];
			resultat = equation_segmenty(point1,point2,evaluation);
			if(resultat <= obstacle_locaux[j][2] && resultat >= obstacle_locaux[j][0])
			{
			  appartient =0;
			}
						
		}
		if(appartient!=0)
		{
			x_vision.push_back(x_cercle[i]);  
			y_vision.push_back(y_cercle[i]);
		}
    }
    appartient = 1;
  }

 
  //Tracer du disque d'obstacle
  for(int i=0; i<x_vision.size();i++)
  {
    x_disque.push_back(init[0]);
    y_disque.push_back(init[1]);
    x_disque.push_back(x_vision[i]);
    y_disque.push_back(y_vision[i]);
  }  
  
  
  
	/*
  if(x_disque.size() !=0)
  {
  g2.set_style("lines");
  g2.plot_xy(x_disque,y_disque,"zone accessible et visible par le robot");
  }
  */
 
  
  //printf("je me dirige vers le points le plus proche de (%f,%f) \n",objectif_courant[0],objectif_courant[1]);
  for(int j=0; j < x_vision.size() ; j++)
  {
    distance_au_goal = (x_vision[j] - objectif_courant[0])* (x_vision[j] - objectif_courant[0]) +  (y_vision[j] - objectif_courant[1])* (y_vision[j] - objectif_courant[1]);
    minimum = min(distance_au_goal,minimum);
    if(minimum ==distance_au_goal)
    {
      inter = j;
    }
  }
  
  
  if(x_vision.size()!=0)
  {
    objectif_int[0] = x_vision[inter];
    objectif_int[1] = y_vision[inter];
    objectif_int[2] = fin[2];
    encombrement = x_vision.size();
  }
  else
	{
    objectif_int[0] = objectif_courant[0];
    objectif_int[1] = objectif_courant[1];
    objectif_int[2] = fin[2];
    encombrement = 0;
	}
  //printf("mon encombrement au moment de la spe est %d\n",encombrement);
return;
}


void communication_voisin(int numero_voisins)
{
	uint8_t * sauvegarde_donnees;
	sauvegarde_donnees = (uint8_t *) malloc(1024 *sizeof(uint8_t));
	double donnes_voisins[10];
	char decimal1[]="000";
	char c_entier1[] = "0";
	char decimal2[]="00";
	char c_entier2[] = "00";
	char decimal3[]="0";
	char c_entier3[] = "000";
	char c_signe[] = "0";
	char c_virgule[] = "0";
	double entier;
	double virgule;
	double signe;
	double signe_atoi;
	
	
	//std::cout << "numero du voisins" << numero_voisins << std::endl;
  
	
	switch(numero_voisins)
	{
 		
	 case 0 :  robot_v1.Read(); op_v1.GetData(sauvegarde_donnees); break;
	 case 1 : robot_v2.Read(); op_v2.GetData(sauvegarde_donnees); break;
	 case 2 : robot_v3.Read(); op_v3.GetData(sauvegarde_donnees); break;
	 /*case 3 : robot_v4.Read(); op_v4.GetData(sauvegarde_donnees); break;
	 case 4 : robot_v5.Read(); op_v5.GetData(sauvegarde_donnees); break;
	 case 5 : robot_v6.Read(); op_v6.GetData(sauvegarde_donnees); break;
	 case 6 : robot_v7.Read(); op_v7.GetData(sauvegarde_donnees); break;
	 case 7 : robot_v8.Read(); op_v8.GetData(sauvegarde_donnees); break;
	 case 8 : robot_v9.Read(); op_v9.GetData(sauvegarde_donnees); break;
	 case 9 : robot_v10.Read(); op_v10.GetData(sauvegarde_donnees); break;
	 case 10 : robot_v11.Read(); op_v11.GetData(sauvegarde_donnees); break;*/
	  default : break;
	}  
	


	for(int i=0;i<=9;i++)
		{
			c_signe[0] = sauvegarde_donnees[6*i];
			
			signe_atoi = atoi(c_signe);
			if(signe_atoi == 1)
			{
			  signe = -1;
			}
			else
			{
			  signe = 1;
			}
			
			c_virgule[0] = sauvegarde_donnees[6*i+5];
			virgule = atoi(c_virgule);
			if(virgule == 1)
			{
			  c_entier1[0] = sauvegarde_donnees[6*i+1];
			  entier = atoi(c_entier1);
			  decimal1[0] = sauvegarde_donnees[6*i+2];
			  decimal1[1] = sauvegarde_donnees[6*i+3];		
			  decimal1[2] = sauvegarde_donnees[6*i+4];		
			  donnes_voisins[i] = signe * ( entier + 0.001 * atoi(decimal1));
			}
			
			if(virgule == 2)
			{
			  c_entier2[0] = sauvegarde_donnees[6*i+1];
			  c_entier2[1] = sauvegarde_donnees[6*i+2];
			  entier = atoi(c_entier2);
			  decimal2[0] = sauvegarde_donnees[6*i+3];
			  decimal2[1] = sauvegarde_donnees[6*i+4];		
			 donnes_voisins[i] = signe * ( entier + 0.01 * atoi(decimal2));
			}
			
			if(virgule == 3)
			{
			  c_entier3[0] = sauvegarde_donnees[6*i+1];
			  c_entier3[1] = sauvegarde_donnees[6*i+2];
			  c_entier3[2] = sauvegarde_donnees[6*i+3];
			  entier = atoi(c_entier3);
			  decimal3[1] = sauvegarde_donnees[6*i+4];		
			  donnes_voisins[i] = signe * ( entier + 0.1 * atoi(decimal3));
			}
			
		
		}
		
	voisins[numero_voisins][0] = donnes_voisins[0];
	voisins[numero_voisins][1] = donnes_voisins[1];
	voisins[numero_voisins][2] = donnes_voisins[2];
	voisins[numero_voisins][3] = donnes_voisins[3];
	voisins[numero_voisins][4] = donnes_voisins[4];
	voisins[numero_voisins][5] = donnes_voisins[5];
	voisins[numero_voisins][6] = donnes_voisins[6];
	voisins[numero_voisins][7] = donnes_voisins[7];
	temps_voisins[numero_voisins][0] = donnes_voisins[8];
	encombrement_vois[numero_voisins] = donnes_voisins[9];
	temps_voisins[numero_voisins][2] = VALEUR_PAS;
	
	
/*
	printf("apres comm x(i+1) =  %f * t(i+1)^3 + %f * t(i+1)^2 + %f * t(i+1) + %f \n", voisins[numero_voisins-1][3],voisins[numero_voisins-1][2],voisins[numero_voisins-1][1],voisins[numero_voisins-1][0]);
	printf("apres comm y(i+1) =  %f * t(i+1)^3 + %f * t(i+1)^2 + %f * t(i+1) + %f \n", voisins[numero_voisins-1][7],voisins[numero_voisins-1][6],voisins[numero_voisins-1][5],voisins[numero_voisins-1][4]);	
	printf("apres comm le temps %f \n",temps_voisins[numero_voisins-1][0]);
	printf("apres comm valeur de l'encombrement %d \n",encombrement_vois[numero_voisins-1]);
*/
	free(sauvegarde_donnees);
return;
}

/* Fonction qui permet de recuperer les trajectoires des voisins */
static void * voisins_voisinages(void * p_data)
{
  for(;;)
  {
  if(nombre_voisins !=0)
  {
    for(int i=0 ; i<nombre_voisins ; i++)
    {
      communication_voisin(i);
    }
  }
  }
  return NULL;
}



/* Permet d'initialiser l'experience */
void	initialisation_experience()
{
  std::cout << "Initialisation de l'experience" << std::endl;
 
  for(int i=0;i<10;i++)
  {
    planif[i] = 0;
  }


  fin_experience = 0;


  //Caracteristique de vitesse
  VMAX=0.8; //Vitesse maximale d'un robot
  VMIN = -VMAX;//Vitesse minimale du robot pendant le parcours pour eviter les singularites
  AMAX = 5;//acceleration maximale d'un robot
  AMIN = -AMIN;//acceleration minimal  d'un robot
  WMAX = 10*M_PI;//Vitesse de rotation maximale d'un robot

  //Caracteristique de temps de la plannification
  
  VALEUR_PAS_APPLICATION = 0.15; // opGetDatapas de temps utilisé
  VALEUR_PAS = 0.15; // pas de temps utilisé
  Nfinal_pred = 80; // l'horizon sur lequel sont évaluées les contraintes
  Nutil = 2; // l'horizon sur lequel est évalué l'objectif ( et qui correspond à la partie appliquée du polynome)
  Nutil_application = 2;

	/*
	//SIMULATION
	VALEUR_PAS_APPLICATION = 0.2; // opGetDatapas de temps utilisé
  VALEUR_PAS = 0.2; // pas de temps utilisé
  Nfinal_pred = 50; // l'horizon sur lequel sont évaluées les contraintes
  Nutil = 25; // l'horizon sur lequel est évalué l'objectif ( et qui correspond à la partie appliquée du polynome)
  Nutil_application = 25;
	*/

	// Distance entre le robot et les obstacles 
  dist_obs = 1;

  // Obstacle circulaire (coordonnées x y du centre du cercle)
 
  nb_obstacle = 0; // nombre d'obstacles à éviter
  coord_obstacle[1][0]=5;
  coord_obstacle[1][1]=19;

  coord_obstacle[0][0]=11.2;
  coord_obstacle[0][1]=25.2;

  coord_obstacle[2][0]=21.5;
  coord_obstacle[2][1]=22.5;


  coord_obstacle[3][0]=4;
  coord_obstacle[3][1]=7;

  coord_obstacle[4][0]=19.5;
  coord_obstacle[4][1]=7.5;

  coord_obstacle[5][0]=24;
  coord_obstacle[5][1]=2.5;


	// Obstacle rectangulaire coordonnées x y du point en bas à droite et x y du point en haut à gauche
  nb_obstacle_rect = 5;
  obstacle_rect[0][0] = 14;
  obstacle_rect[0][1] = 14;
  obstacle_rect[0][2] = 22.5;
  obstacle_rect[0][3] = 17.5;
  
  obstacle_rect[1][0] = -11+13;
  obstacle_rect[1][1] = 11+13;
  obstacle_rect[1][2] = 4.5;
  obstacle_rect[1][3] = 13.5+13;
  
  
  obstacle_rect[2][0] = -5+13;
  obstacle_rect[2][1] = 1.5+13;
  obstacle_rect[2][2] = -3+13;
  obstacle_rect[2][3] = 3.5+13;
  
  
  obstacle_rect[3][0] = 13;
  obstacle_rect[3][1] = 6.5;
  obstacle_rect[3][2] = 15;
  obstacle_rect[3][3] = 8.5;
  
  obstacle_rect[4][0] = 14;
  obstacle_rect[4][1] = 14;
  obstacle_rect[4][2] = 17.5;
  obstacle_rect[4][3] = 23;

 
  
  
  
  // Nombre de point sur le cercle représentatif de la zone accessible au robot
  nombre_de_point = 360;

  

	// Coordonnées de l'objectif final
 
	fin[0]=finx;
  fin[1]=finy;
  fin[2]=VMAX;



  // Informations sur les voisins
  nombre_voisins=3; // nombre de voisins à éviter
  dist_robot_min = 1; // distance minimale inter robot


	// Coopération (pas nécessaire pour l'intant
  encombrement =0;



	// Variable pour la comm avec les voisins.
  opGetData = (uint8_t *) malloc(2048 *sizeof(uint8_t));
  opData.data = (uint8_t *) malloc(2048 *sizeof(uint8_t));

  return;
}

static void * fn_tracer_poly(void * p_data)
{	
  // Copy of the current polynom to avoid corruption of data
  double * variable_intermedaire = ( double * ) p_data;
  double *polynome_courant = new double[9];
  double temps,last_x,last_y,last_xv,last_yv,last_xv2,last_yv2;;
  std::vector<double> x_obstacle,y_obstacle;
  
  for(int i=0;i<=8;i++)
  {
    //polynome_courant[i] = planif[i];
    polynome_courant[i] = variable_intermedaire[i];
  }


  g1.reset_plot();
  
  for (int i = 0; i <= Nutil; i++)  
  {
    temps = i * VALEUR_PAS;  
    last_x = polynome_courant[0] + polynome_courant[1]*temps +polynome_courant[2]*temps*temps + polynome_courant[3]*temps*temps*temps;  
    x.push_back(last_x);
    last_y = polynome_courant[4] + polynome_courant[5]*temps+polynome_courant[6]*temps*temps + polynome_courant[7]*temps*temps*temps; 
    y.push_back(last_y);
    
    if(nombre_voisins == 1)
    {
    last_xv = voisins[0][0] + voisins[0][1]*temps +voisins[0][2]*temps*temps + voisins[0][3]*temps*temps*temps;  
    xv.push_back(last_xv);
    last_yv = voisins[0][4] + voisins[0][5]*temps+voisins[0][6]*temps*temps + voisins[0][7]*temps*temps*temps; 
    yv.push_back(last_yv);
    /*
    last_xv2 = voisins[1][0] + voisins[1][1]*temps +voisins[1][2]*temps*temps + voisins[1][3]*temps*temps*temps;  
    xv2.push_back(last_xv2);
    last_yv2 = voisins[1][4] + voisins[1][5]*temps+voisins[1][6]*temps*temps + voisins[1][7]*temps*temps*temps; 
    yv2.push_back(last_yv2);
    */
  	}

  }
  g1.set_style("lines");
  if(nombre_voisins ==1 && xv.size()!=0)
  {
    g1.plot_xy(xv,yv,"trajectoire du robot voisin 1");
  }
  g1.plot_xy(x,y,"trajectoire du robot courant");
  delete[] polynome_courant;
  
  for(int i=0 ; i<nb_obstacle_rect ; i++)
  {
    x_obstacle.push_back(obstacle_rect[i][0]);
    y_obstacle.push_back(obstacle_rect[i][1]);
    
    x_obstacle.push_back(obstacle_rect[i][0]);
    y_obstacle.push_back(obstacle_rect[i][3]);
    
    x_obstacle.push_back(obstacle_rect[i][2]);
    y_obstacle.push_back(obstacle_rect[i][3]);
    
    x_obstacle.push_back(obstacle_rect[i][2]);
    y_obstacle.push_back(obstacle_rect[i][1]);
    
    x_obstacle.push_back(obstacle_rect[i][0]);
    y_obstacle.push_back(obstacle_rect[i][1]);
    
    x_obstacle.push_back(NULL);
    y_obstacle.push_back(NULL);
  }
  
  g1.set_style("lines");
  if(x_obstacle.size()!=0)
  {
      g1.plot_xy(x_obstacle,y_obstacle,"obstacles");
  }
  return NULL;

}

/* Fonction lance par un thread permettant de recuperer la position du robot en continue et qui enregistre cette infos dans la variable position_robot */
static void * fn_position(void * p_data)
{
	
	float xrep = XREP;
	float yrep = YREP;
	
	double x,y,theta,vx,vy;
	
		for(;;)
		{
		robot.Read();
		pthread_mutex_lock (& mutex_position);
		x=pp_pos.GetXPos();
		y=pp_pos.GetYPos();
		theta=pp_pos.GetYaw();
		pthread_mutex_unlock (& mutex_position);
		position_robot[0]= -xrep+ debutx + x;
		position_robot[1]=-yrep + y + debuty;
		position_robot[2]=theta;
		sleep(0.01);
		}
	return NULL;
}			

static void * fn_recup_poly(void * p_data)
{
// Pour communiquer mon polynome à mes voisins
  double result[6]={0,0,0,0,0,0};
  char buf[]="0";
  double intermediaire;
  // Copy of the current polynom to avoid corruption of data
  
  
  double * variable_intermedaire = ( double * ) p_data;
  double *polynome_courant = new double[10];
  
  //printf("dans la fonction d'envoie de data %d \n",encombrement);
  
  for(int i=0;i<=9;i++)
  {
    polynome_courant[i] = 0;
  }
  
  for(int i=0;i<=8;i++)
  {
    polynome_courant[i] = variable_intermedaire[i];
  }
  
  polynome_courant[9] = encombrement;
  
  
  for(int i=0;i<=9;i++)
  {
    intermediaire = polynome_courant[i];
    decompose(intermediaire,result);

  
    sprintf(buf, "%f",result[0]);
    opData.data[6*i]=buf[0];
    
    sprintf(buf, "%f",result[1]);
    opData.data[6*i+1]=buf[0];

    sprintf(buf, "%f",result[2]);
    opData.data[6*i+2]=buf[0];

    sprintf(buf, "%f",result[3]);
    opData.data[6*i+3]=buf[0];

    sprintf(buf, "%f",result[4]);
    opData.data[6*i+4]=buf[0];

    sprintf(buf, "%f",result[5]);
    opData.data[6*i+5]=buf[0];
  }

  opData.data_count = 60; 
  op.SendCmd(&opData); 
  return NULL;
}

/* Fonction lance par un thread qui permet de commande le robot en vitesse en parallel du calcul d'un nouveau polynome*/
static void * fn_command (void * p_data)
{ 	
	struct timeval tv1,tv2;
	struct timezone tz;
  	long long diff;
  	long long valeur_attente;
  	int Nutil_final=Nutil_application;
  	double vitesse_lin[Nutil_application];
	double vitesse_ang[Nutil_application];
	double vitesse_x[Nutil_application];
	double vitesse_y[Nutil_application];
	double acc_x[Nutil_application];
	double acc_y[Nutil_application];
	double position_final[4];


	// Copy of the current polynom to avoid corruption of data
	double * variable_intermedaire = ( double * ) p_data;
	double *polynome_courant = new double[9];
	for(int i=0;i<9;i++)
	{
	polynome_courant[i] = variable_intermedaire[i];
	//printf("polynome courant %f \n",polynome_courant[i]);
	}
	
	
	// We check if the objectif would be atteign by the robot during the current trajectory
	for(int i=0; i<= Nutil_application; i++)
	{	
		position_final[0] = polynome_courant[0] + polynome_courant[1]*VALEUR_PAS_APPLICATION*i +polynome_courant[2]*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i) + polynome_courant[3]*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i);  
		position_final[1] = polynome_courant[4] + polynome_courant[5]*VALEUR_PAS_APPLICATION*i +polynome_courant[6]*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i) + polynome_courant[7]*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i);  
	
	
		//if(fabs(fin[0] -position_final[0]) <0.1 && fabs(fin[1] -position_final[1]) < 0.1)
		if(sqrt((fin[0] -position_final[0])*(fin[0] -position_final[0]) + (fin[1] -position_final[1])*(fin[1] -position_final[1]))<0.3)
		{
			Nutil_final = i;
			fin_experience = 1;
			printf("The objectif would be atteign by the robot during this trajectory\n"); 
			break;
		} 
	}
	
	
	for(int i=0;i<=Nutil_final;i++)
	{
		vitesse_x[i] = polynome_courant[1] + 2*polynome_courant[2]*VALEUR_PAS_APPLICATION*i +3*polynome_courant[3]*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i);  
		vitesse_y[i] = polynome_courant[5] + 2*polynome_courant[6]*VALEUR_PAS_APPLICATION*i +3*polynome_courant[7]*(VALEUR_PAS_APPLICATION*i)*(VALEUR_PAS_APPLICATION*i);
		acc_x[i] = 2*polynome_courant[2] + 6*polynome_courant[3]*(VALEUR_PAS_APPLICATION*i);
		acc_y[i] = 2*polynome_courant[6] + 6*polynome_courant[7]*(VALEUR_PAS_APPLICATION*i);

		vitesse_lin[i]=sqrt(vitesse_x[i]*vitesse_x[i]+vitesse_y[i]*vitesse_y[i]);
		if(((vitesse_x[i]!=0.0)||(vitesse_y[i]))!=0.0)
		{	
			vitesse_ang[i]=(acc_y[i]*vitesse_x[i]-acc_x[i]*vitesse_y[i])/(vitesse_x[i]*vitesse_x[i]+vitesse_y[i]*vitesse_y[i]);
			vitesse_ang[i]=vitesse_ang[i]/coeff;
			//std::cout << "valeur de la vitesse" << vitesse_ang[i] << std::endl;
		}
		else
		{
			vitesse_ang[i]=0.0; 
		}
	}
	
	if(fin_experience == 1)
	{
	  vitesse_lin[Nutil_final] = 0;
	  vitesse_ang[Nutil_final] = 0;
	}
	gettimeofday(&tv1, &tz);
	for(int i=0;i<=Nutil_final;i++)
	{	pthread_mutex_lock (& mutex_position);
		//std::cout << "valeur de i" << i << "vitesse lineaire" << vitesse_lin[i] <<"vitesse angulaire" << vitesse_ang[i] << std::endl;
		pp_commande.SetSpeed(vitesse_lin[i],vitesse_ang[i]);
		pthread_mutex_unlock (& mutex_position);
		
		gettimeofday(&tv2, &tz);
	  diff=(tv2.tv_sec-tv1.tv_sec) * 1000000L + (tv2.tv_usec-tv1.tv_usec);  
		valeur_attente = i*VALEUR_PAS_APPLICATION * 1000000L  - diff;
		if(valeur_attente >= 0)
		{
			usleep(valeur_attente);
		}
		else if(valeur_attente < 0 && i!=0)
		{
		std::cout << valeur_attente *0.000001 << "            " << "There are not enough time to work on the robot!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
		}
 	}
 	
 	
 	delete[] polynome_courant;
	return NULL;
}



/* The main function of the program */
int main(int argc, char **argv)
{
  double init[4];
  double objectif_int[3]={0,0,0};
  double objectif_courant[3] = {0,0,0};
  double previous_polynome[9]={0,0,0,0,0,0,0,0,0};
  int inform;
  int ret_application=1;	// variable pour la creation des thread
 	int ret_position = 1;
  int ret_tracer = 1;
  int ret_communication = 1;
  int ret_voisin =1;
  pthread_t thread_application;
  pthread_t thread_tracer;
  pthread_t thread_position;
  pthread_t thread_communication_voisin;
  pthread_t thread_recup_voisin;
  double vitesse_x_final=0;
  double vitesse_y_final=0;
  double final[4]={0,0,0,0};		
  double temps;
  int premier_tour=1;
  char fermeture;
  double goal_vision;
  double vision= VISION_ROBOT;
  int cooperation_vois= COOPERATION;
  
  double essais[4]={1,1,0,0};


  struct timeval tv1,tv2;
  struct timezone tz;
  double diff;

  gettimeofday(&tv1, &tz);

  

  try
  {
    // Print of the robot information at the beginning
    std::cout << robot << std::endl;
    pp_commande.SetMotorEnable (true);
    pp_commande.SetSpeed(0,0);
    robot.Read();

    initialisation_experience();

    
    
    // Thread qui permet de récupérer en boucle la position du robot.
	ret_position = pthread_create(& thread_position, NULL,fn_position,NULL);
	// Thread qui permet de communiquer en boucle entre le robot et ses voisins pour récupérer leur trajectoire. 
	ret_voisin = pthread_create(& thread_recup_voisin, NULL,voisins_voisinages,NULL);
	  
	  
    for(;;)
    {	

			
    	
      // Permet de récupérer la position courante du robot.
      init[0]=position_robot[0];
      init[1]=position_robot[1];
      init[2]=position_robot[2];
      
      
      // Distance initial entre le robot et son objectif pour la coopération pas nécessaire pour le moment
			if(premier_tour != 0)
			{      
       distance_initiale = sqrt((init[0] - fin[0])*(init[0] - fin[0]) + (init[1] - fin[1])*(init[1] - fin[1]));
       init[3] = 0.01;
			}
      

      // Calcul of the last point of the current trajectory which will be use as start point for the next trajectory
      temps = VALEUR_PAS*(Nutil);

      vitesse_x_final = previous_polynome[1] + 2*previous_polynome[2]*temps +3*previous_polynome[3]*temps*temps;  
      vitesse_y_final = previous_polynome[5] + 2*previous_polynome[6]*temps +3*previous_polynome[7]*temps*temps;
      final[3] = sqrt(vitesse_x_final*vitesse_x_final + vitesse_y_final*vitesse_y_final);

      if(premier_tour == 0)
      {  
				init[3]=final[3];
				if(init[3]==0)
				{
					init[3] = 0.01;
				}
      }
       
      objectif_courant[0] = fin[0];
      objectif_courant[1] = fin[1];
      objectif_courant[2] = fin[2];
  
  
  		// L'objectif final est-il dans la zone d'accessibilité du robot.
      goal_vision = sqrt((init[0] - fin[0])*(init[0] - fin[0]) + (init[1] - fin[1])*(init[1] - fin[1]));
      
      
			/*     
			if(nombre_voisins != 0 && premier_tour == 0 && goal_vision > vision && cooperation_vois == 1)
      {
				cooperation(objectif_int);
      }
      else
      {
				objectif_courant[0]=fin[0];
				objectif_courant[1]=fin[1];
				objectif_courant[2]=fin[2];
      }
   		*/
      
		
		
      // Fonction qui donne un objectif intermediaire si obstacle rectangulaire.


			cooperation(objectif_courant);

      obstacles_voisinages(init,objectif_int,objectif_courant,0.3);

      // Calcul de la trajectoire en prenant en compte les contraintes, le point initial et l'objectif intermediaire.
      inform = optimisation(init, objectif_int,previous_polynome);
     
      gettimeofday(&tv2, &tz);
      diff=(tv2.tv_sec-tv1.tv_sec)*1000000L + (tv2.tv_usec-tv1.tv_usec);  	
      planif[8]  = diff/1000000L;
      
      printf("le robot par de (%f,%f) pour aller vers (%f,%f,%f), la planification est %d \n",init[0],init[1], objectif_int[0],objectif_int[1],objectif_int[2],inform);
      printf("le temps %f \n",planif[8]);
     
      for(int i = 0; i<=8 ; i++)
      {
				previous_polynome[i] = planif[i];
      }
     
     
      if(inform ==2)
      {
				for(int i = 0; i<=8 ; i++)
				{
					previous_polynome[i] = 0;
				}
				previous_polynome[1] = 0.1;
				previous_polynome[5] = 0.1;
      }

      // Pour appliquer la trajectoire sur le robot
      if(ret_application == 0)
      {
			pthread_join (thread_application, NULL);
      }
      ret_application = pthread_create(& thread_application, NULL,fn_command, (void *) previous_polynome);
 
      
      // Pour donner notre trajectoire au voisin
      if(nombre_voisins !=0)
      {
				if(ret_communication == 0)
				{					
					pthread_join (thread_communication_voisin, NULL);
				}		
				ret_communication = pthread_create(& thread_communication_voisin, NULL,fn_recup_poly, (void *) previous_polynome);
      }
 


		
			// Pour tracer notre trajectoire 
			/*      
			if(ret_tracer == 0)
      {
     			pthread_join (thread_tracer, NULL);
      }		
      ret_tracer = pthread_create(& thread_tracer, NULL,fn_tracer_poly, (void *) previous_polynome);
     	*/
 			//printf("valeur de fin experience %d \n",fin_experience);
      
     /*

			if(fin_experience == 1)
      {				
				if(ret_application == 0)
		    {
				pthread_join (thread_application, NULL);
		    }
		    


		    if(ret_tracer == 0)
		    {
		   	pthread_join (thread_tracer, NULL);
		    }	
		    
		    if(ret_communication == 0)
				{
					pthread_join (thread_communication_voisin, NULL);
				}		

			pp_commande.SetSpeed(0,0);
			for(int i = 0; i<=8 ; i++)
			{
				previous_polynome[i] = 0;
			}
			previous_polynome[0] = fin[0];
			previous_polynome[4] = fin[1];
		  break;
      }
      */
      premier_tour=0;
    }


   	for(;;)
   	{
   //	printf("arret du robot et communication avec les voisins \n");
    ret_communication = pthread_create(& thread_communication_voisin, NULL,fn_recup_poly, (void *) previous_polynome);
    if(ret_communication !=0)
    	{pthread_join (thread_communication_voisin, NULL);}
    sleep(2);
    }
    wait_for_key();
    delete[] opData.data;
    delete[] opGetData; 

  } 
  catch (PlayerCc::PlayerError e)
  {
  std::cerr << e << std::endl;
  return -1;
  }
}
